#ifndef ROBOT_H
#define ROBOT_H

#include "models/command.h"
#include "databasemanager.h"
#include <QObject>
#include <QTcpSocket>
#include <QUdpSocket>
#include <QTimer>
#include <QJsonObject>
#include <QJsonDocument>
#include <QJsonArray>
#include <QHostAddress>
#include <QVector>
#include <QString>
#include <QByteArray>
#include <QImage>
#include <QWebSocket>
#include <QAudioInput>
enum ToolType {
    TOOL_NONE =0,
    TOOL_HAND,
    TOOL_GRIPPER,
    TOOL_SUCTION_CUP
};
enum HeadType{
    HEAD_NORMAL = 0,
    HEAD_SENTRY, //哨兵头
    HEAD_SLAM //感知头
};


struct SentryDestInfo{
    int nX;
    int nY;
    int nW;
    int nH;
    float fDegree;
};


#pragma pack(1)

//UDP控制命令
struct SportCommand{
    unsigned int    id;                 //ID
    unsigned short  dataNum;            //字节数量
    unsigned char   ctrMode = 0;            //操控模式 0/1/2
    float           turnSpeed = 0;          //转向速度
    float           xSpeed = 0;             //x方向速度
    float           ySpeed = 0;             //y方向速度
    int             reserve1[8];        //预留
    float           height = 0;        //高度
    float           speed = 0;          //速度
    float           destance = 0;       //距离
    float           angle = 0;          //角度
    float           barycenter = 0;     //重心
    float           leftT = 0;           //LT
    float           rightT = 0;           //RT
    float           reserve = 0;        //预留
    unsigned char   cmdType[3]={0};         //0-步态号  1-指令号
    unsigned char   reserve3[10]={0};       //预留
};

//UDP返回机器人状态
typedef struct SportState{
    float expArc[31]={0};               //期望角度
    float realArc[31]={0};                 //实际角度
    float expMoment[31]={0};             //期望力矩
    float realMoment[31]={0};            //实际力矩
    float expSpeed[31]={0};             //期望速度
    float realSpeed[31]={0};             //实际速度
    float state[31]={0};            //状态字
    float error[31]={0};            //错误子
    float tempture[31]={0};             //温度
    unsigned char szWarning[31]={0};     //告警信息
    float gyroAngle[3]={0};                 //陀螺角度
    float batteryLevel;                 //电池电量
    short curTread;                     //当前步态
    short curTreadStat;                 //当前步态状态
    short curCtrMode;                   //当前控制模式
    float forwardSpeed;                 //行进速度
    float descPos[2]={0};                  //目标点位置
    float persionPos[2]={0};                 //人员位置
    unsigned char mapTag;               //网格地图标志
    unsigned char gyroStat;             //陀螺状态
    unsigned char powerStat[4]={0};     //电池状态
    unsigned char reserve1[4]={0};  //预留字段
    short reserve2[4]={0};  //预留字段
    float gps[3]={0};  //GPS坐标
    float reserve3;
}SportState;

#pragma pack()

//标零协议相关枚举
//报文标识
enum Portocal_Mask{
    Portocal_Mask_Robot = 1,
    Portocal_Mask_Controller = 2
};
//信息分类ID
enum Protocal_Type{
    Protocal_Type_MidderWare = 0x10,
    Protocal_Type_interface = 0x20,
    Protocal_Type_daemo = 0x30,
    Protocal_Type_Controller = 0x40
};
//数据类型ID
enum Protocal_DataType{
    Protocal_DataType_State = 0,
    Protocal_DataType_Pair = 1,
    Protocal_DataType_Zero = 2,
    Protocal_DataType_Thread = 3
};

class Robot : public QObject
{
    Q_OBJECT
public:
    explicit Robot(QObject *parent = nullptr);
    Robot(int nID,QString strName,QString strUrdfName,
          QString strSportIP,
          int nSportPort,
          QString strPerceptionIP,
          int nPerceptionrPort,
          QString strSlamIP,
          int nSlamPort,
          QString strSpeakIP,
          int nSpeakPort,
          QString strZeroIP,
          int nZeroPort,
          QString strTelemetryIP,
          int nTelemetryPort,
          ToolType leftTool,
          ToolType rightTool,
HeadType headType,
          QString strUser,
          QString strPwd,
          QList<VideoInfo> lsVideo);
    ~Robot();
    QString urdfName();
    // 连接控制
    void connectRobot();
    void disconnectRobot();

    //是否有感知
    bool hasPerception();
    //是否有Slam
    bool hasSlam();
    //是否有喊话
    bool hasSpeak();
    //是否有标零
    bool hasZero();
    //是否有数采
    bool hasTelemetry();
    //请求控制
    bool requestControl(bool control, QString & strErr);
    //手动模式下发送控制上肢关节指令信息（TCP）
    bool startSetJoint(JointType type,int nId,float value);
    bool stopSetJoint();
    //手动模式下发送控制末端执行器指令信息（TCP）
    // bool setToolPos(float left_rx,float left_ry,float left_rz,float left_x,float left_y,float left_z,float left_grasp,
    //                 float right_rx,float right_ry,float right_rz,float right_x,float right_y,float right_z,float right_grasp);
    //示教模式下发送节点信息（TCP）
    bool startDemonstrate(QString jsonData,int times);
    //停止示教
    bool stopDemonstrate();
    bool setTurn(float fTurn);
    bool setMove(float fX, float fY);
    //设置指令值
    bool setUpperCmd(Command cmd);
    bool setGlobalCmd(Command cmd);
    void setEnable(bool bEnable);    
    //哨兵头

    void sentryIndify(int nType);//识别
    void sentryTrack(int nVideo);//追踪
    void sentryGetResult();//获取识别追踪信息
    void sentryDestance(); //测距

    void setAudioFlie(AudioFile file);
    //SLAM相关
    bool getMap();
    bool initPosition(QPointF position, float angle);
    bool startPosition();
    bool stopPosition();
    bool startPointCloud();
    bool stopPointCloud();
    bool startNavigatione(QPointF dest, float angle);
    bool stopNavigatione();
    //数采
    bool startTelemetry();
    bool stopTelemetry();
    //标零
    bool zeroJoints( QVector<int> lsJointIndex);
    //视频
    QList<VideoInfo> lsVideo() const;

    void update(QString strName,QString strUrdfName,
                QString strSportIP,
                int nSportPort,
                QString strPerceptionIP,
                int nPerceptionrPort,
                QString strSlamIP,
                int nSlamPort,
                QString strSpeakIP,
                int nSpeakPort,
                QString strZeroIP,
                int nZeroPort,
                QString strTelemetryIP,
                int nTelemetryPort,
                ToolType nLeftTool,
                ToolType nRightTool,
                HeadType headType,
                QString strUser,
                QString strPwd,
                QList<VideoInfo> lsVideo);

    // QString currentGait() const;

    QString strWarning() const;

    void setVoiceCmdText(QString strText);//处理语音识别命令
    int nID() const;
    QString strName() const;

    ToolType leftTool() const;

    ToolType rightTool() const;

    QVector<float> getJointsArc(JointType type);

    SportState SportStateData();

    int nUpperCmdID() const;

    QList<int> lsWnd() const;

    void setLsWnd(const QList<int> &newLsWnd);

    float nRelolution() const;

    QImage *pImgMap();

    float mapRes() const;

    QPointF mapOrigin() const;

    QPointF mapRobot() const;

    float mapRobotAngle() const;

    QImage *pImgPointCloud() ;

    QPointF ptPointCloud() const;

    int nGlobalCmdID() const;

    void setPage(TabType page);
    void setTeleoperation(int nTeleoperation);
    void startTeleoperation();
    void stopTeleoperation();

    bool OpenSpeaker();
    bool closeSpeaker();


    QVector<float> leftTip() const;

    QVector<float> rightTip() const;

    QVector<float> leftFingerArc() const;

    QVector<float> leftFingerArcExp() const;

    QVector<float> leftFingerMoment() const;

    QVector<float> leftFingerMomentExp() const;

    QVector<float> leftFingerSpeed() const;

    QVector<float> leftFingerSpeedExp() const;

    QVector<float> rightFingerArc() const;

    QVector<float> rightFingerArcExp() const;

    QVector<float> rightFingerMoment() const;

    QVector<float> rightFingerMomentExp() const;

    QVector<float> rightFingerSpeed() const;

    QVector<float> rightFingerSpeedExp() const;

    int nAudioFileID() const;

    QString strSportIP() const;

    int nSportPort() const;

    QString strPerceptionIP() const;

    int nPerceptionrPort() const;

    QString strZeroIP() const;

    int nZeroPort() const;

    QString strTelemetryIP() const;

    int nTelemetryPort() const;

    QString strSlamIP() const;

    int nSlamPort() const;

    QString strSpeakIP() const;

    int nSpeakPort() const;

    HeadType headType() const;

    void setSentryOptical(int nFunc, int nMode, int nParam);
    void setSentryLaser();

    QString strUser() const;

    QString strPassword() const;
    void disConnectTelemetryWS();
    bool connectTelemetryWS(QString &strErr);
    void setStartCmd();
    void setStopCmd();
    void setJoystickState(bool bState){m_bJoystickState=bState;}

    int getNStage() const;

signals:
    void controlStateChanged(bool controlled);
    void toolStateChanged();
    void batteryChanged();
    void telemetryChanged(bool bTelemetry);
    void telemetryResult(QString result);
    void gaitChanged(const QString& gait);
    void errorOccurred(const QString& error);
    void trainingModeChanged(bool enabled, const QString& mode);
    void specificMoveCompleted(const QString& moveName);
    void jointControlCompleted(bool success);
    void sportStateChanged();
    void warningChanged(QString strWarning);
    void upperCmdChanged();
    void globalCmdChanged();
    void audioFileChanged();
    void mapChanged();
    void pointCloudChanged();
    void robotPositionChanged();
    void speakWSError();
    void telemetryWSError();
    void sentryIndifyChanged(float fieldangle,float undershootX, float undershootY,QVector<SentryDestInfo> vtDest);
    void sentryDistanceChanged(int destType,int destNum, float distance);
    void sentrySoundChanged(int angle);
    void tempratureTooHighChanged(QList<QString> lsTemprature);
    void stageChanged();

public slots:
    void onPerceptionTCPReadyRead();
    void onPerceptionUDPReadyRead();
    void onSportUDPReadyRead();
    void onSlamUDPReadyRead();
    void onTelemetryWSReceived(const QString &message);
    void onPerceptionTCPError(QAbstractSocket::SocketError error);
    void onPerceptionTCPConnect();
    void onSpeakWSError(QAbstractSocket::SocketError error);
    void onTelemetryWSError(QAbstractSocket::SocketError error);
    void onSportSendTimer();
    void onSportRecvTimer();
    void onStickKeyPressed(FUNCTION_KEY nFuncKey, STICK_KEY nKey);
    void onStickXChanged(float x);
    void onStickYChanged(float y);
    void onSticUChanged(float u);
    void onStickRChanged(float r);
    void onStickZChanged(float z);

protected:
    bool connectPerceptionTCP(QString & strErr);
    bool connectPerceptionUDP(QString & strErr);
    bool connectSportUDP(QString & strErr);
    bool connectSlamUDP(QString & strErr);
    bool connectZeroUDP(QString &strErr);
    bool connectSpeakWS(QString & strErr);

    void disConnectPerceptionTCP();
    void disConnectPerceptionUDP();
    void disConnectSportUDP();
    void disConnectSlamUDP();
    void disConnectZeroUDP();
    void disConnectSpeakWS();

    // 添加JSON解析函数
    QList<QJsonDocument> parseJsonFromBuffer(QByteArray &buffer);
    //解析感知返回的数据
    void processPerception(const QJsonDocument& jsonDoc);
    //解析感知返回的数据
    void processSlam(const QJsonDocument& jsonDoc);
    //检查告警
    void checkWarning(SportState oldState, SportState newState);
    void checkWarningByType(JointType type,SportState oldState, SportState newState);
    //
    void onReGetMapTimer();
    bool sendZeroJoint( QByteArray arrData);

    //设置指令值
    bool setCmd(int nCmd);

private:
    QTcpSocket* m_sockPerceptionTCP;
    QUdpSocket* m_sockPerceptionUDP;
    QUdpSocket* m_sockSportUDP;
    QWebSocket * m_sockTelemetryWS;
    QWebSocket * m_sockSpeakWS;
    QUdpSocket * m_sockZeroUDP;
    QUdpSocket* m_sockSlamUDP;

    //左手位置 顺序为 rzryrxXYZ,最后一个是没有用的臂展角
    QVector<float> m_leftTip;
    QVector<float> m_leftTipExp;
    //右手位置
    QVector<float> m_rightTip;
    QVector<float> m_rightTipExp;

    //左手手指 手部关节数据，数组内为6个自由度的位姿，如果是夹爪和吸盘，使用第二个元素，即m_leftFinger[1]
    QVector<float> m_leftFingerArc;
     QVector<float> m_leftFingerArcExp;
    QVector<float> m_leftFingerMoment;
    QVector<float> m_leftFingerMomentExp;
    QVector<float> m_leftFingerSpeed;
    QVector<float> m_leftFingerSpeedExp;
    //右手手指
    QVector<float> m_rightFingerArc;
    QVector<float> m_rightFingerArcExp;
    QVector<float> m_rightFingerMoment;
    QVector<float> m_rightFingerMomentExp;
    QVector<float> m_rightFingerSpeed;
    QVector<float> m_rightFingerSpeedExp;

    SportCommand m_SportCmd;

    SportState m_SportState;

    //16进制发送定时器
    QTimer m_sportSendTimer;
    //16进制超时时间
    QTimer m_sportRecvTimer;
    //第一次接到hex数据
    bool m_bRecvHex;
    //重新获取地图定时器，防止丢包导致地图无法加载
    QTimer m_reGetMapTimer;

    //机器人ID
    int m_nID;
    //机器人名称
    QString m_strName;
    //机器人urdf
    QString m_strUrdfName;
    //连接信息
    QString m_strSportIP;
    int m_nSportPort;
    QString m_strPerceptionIP;
    int m_nPerceptionrPort;
    QString m_strSlamIP;
    int m_nSlamPort;
    QString m_strSpeakIP;
    int m_nSpeakPort;
    QString m_strZeroIP;
    int m_nZeroPort;
    QString m_strTelemetryIP;
    int m_nTelemetryPort;
    bool m_bJoystickState;

    // 左工具和右工具变量
    ToolType m_leftTool;
    ToolType m_rightTool;
    HeadType m_headType;
    //ssh用户名密码
    QString m_strUser;
    QString m_strPassword;

    //视频
    QList<VideoInfo> m_lsVideo;

    // tcp添加接收缓冲区
    QByteArray m_tcpPerceptionBuffer;

    QString m_strWarning;
//    //毛辰飞增加，关节过热
    QList<QString> m_lsJointTooHigh;

     int m_nGlobalCmdID;
     int m_nUpperCmdID;
    int m_nAudioFileID;

    QImage m_imgMap;     //地图图片
    QImage m_imgPointCloud;
    float  m_nMapRes;    //地图分辨率
    QPointF m_ptMapOrigin; //地图原点
    QPointF m_ptMapRobot; //机器人坐标
    QPointF m_ptPointCloud;//感知起点位置
    float m_fMapRobotAngle = 0;//机器人角度
    //stage
    int m_nStage = 0;
    //当前所在页
    TabType m_nCurPage;
    int m_nTelemetryMid;
    QMap<int,QString> m_mpTelemetryCmd;

//感知相关结构
#pragma pack(1)
    struct Slam_msg{
        unsigned char szHeader;
        unsigned char szIndex;
        unsigned short nPkCount;
        unsigned short nPkIndex;
        unsigned short nLen;
        unsigned char szData[]; // Flexible array member
    };
#pragma pack()

    struct SLAM_PACKAGES{
        unsigned char szCmd;
        unsigned short nPkCount;
        QMap<int,QString>dataList;
    };

    QMap<int,SLAM_PACKAGES> m_mpSlamRecv;



    QAudioInput *m_pAudioInput = nullptr;
    QIODevice *m_pAudioInputDevice = nullptr;

    QJsonObject m_setJointCmd;

};
#endif // ROBOT_H
